#include <iostream>
#include <cmath>
#include <vector>
#include <numeric>

/**
 * @file fusion.hpp
 * @author Liuxc
 * @brief 双数据融合算法 对数据源进行状态观测 进行动态置信度调节
 */

using namespace std; // 使用命名空间std

namespace faf // filter add fusion namespace
{
    typedef struct
    {
        // 平方差和
        float sum_square = 0.0;
        // 均值
        float mean = 0.0;
        // 方差
        float variance = 0.0;
        // 标准差
        float std_dev = 0.0;
        // 预测值
        float predicting = 0.0;
        // 残差 实际值与估计值之间的差
        float residual = 0.0;
        // 拟合优度 即线性度 约靠近1 线性程度越好
        float r_squared = 0.0;
    } data_stauts_t; // 数据状态

    class fusion
    {
    private:
#define BUFFER_SIZE 3

        float a_buffer[BUFFER_SIZE], b_buffer[BUFFER_SIZE];
        float *a_ptr = a_buffer; //
        float *b_ptr = b_buffer; //

        float output = 0.0; // 输出值

        data_stauts_t data_stauts_a, data_stauts_b; // 数据状态

        float processNoise;     // 过程噪声
        float measurementNoise; // 测量噪声
        float estimatedError;   // 估计误差
        float currentEstimate;  // 当前估计值
        /***
         * 写入数据到缓冲区
         */
        int write_a(float a)
        {
            if (a_ptr == a_buffer + BUFFER_SIZE) // 如果指针指向a_buffer的末尾，则重新指向a_buffer的起始位置
            {
                a_ptr = a_buffer;
            }
            *a_ptr = a; // 写入数据
            a_ptr++;    // 指针指向下一个位置
            return 0;
        }
        /**
         * 写入数据到缓冲区
         */
        int write_b(float b)
        {
            if (b_ptr == b_buffer + BUFFER_SIZE) // 如果指针指向b_buffer的末尾，则重新指向b_buffer的起始位置
            {
                b_ptr = b_buffer;
            }
            *b_ptr = b; // 写入数据
            b_ptr++;    // 指针指向下一个位置
            return 0;
        }

        void read_a(float *a)
        {
            for (size_t i = 0; i < BUFFER_SIZE; i++)
            {
                if (a_ptr + i >= a_buffer + BUFFER_SIZE)
                {
                    *(a + i) = *(a_ptr + i - BUFFER_SIZE); // 如果指针指向a_buffer的末尾，则重新指向a_buffer的起始位置
                }
                else
                {
                    *(a + i) = *(a_ptr + i); // 读取数据
                }
            }
        }

        void read_b(float *b)
        {
            for (size_t i = 0; i < BUFFER_SIZE; i++)
            {
                if (b_ptr + i >= b_buffer + BUFFER_SIZE)
                {
                    *(b + i) = *(b_ptr + i - BUFFER_SIZE); // 如果指针指向b_buffer的末尾，则重新指向b_buffer的起始位置
                }
                else
                {
                    *(b + i) = *(b_ptr + i); // 读取数据
                }
            }
        }

        /**
         * 计算数据状态 以及线性化
         * @param data 数据点
         * @param len 数据点数量
         * @param data_stauts 数据状态结构体
         * @return 执行结果
         *          */
        int calculateLinearity(const float *data, int len, data_stauts_t *data_stauts)
        {
            int n = len; // 数据长度
            float sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_x2 = 0.0;
            float slope;     // 斜率
            float intercept; // 截距
            float mean_y;    // 均值

            // 计算必要的和
            for (int i = 0; i < n; ++i)
            {
                sum_x += i;            // x 为序列索引
                sum_y += data[i];      // y 为数据值 数据和
                sum_xy += i * data[i]; // xy 为数据索引与数据值的乘积
                sum_x2 += i * i;       // x^2 为数据索引的平方
            }

            // 计算斜率（slope）和截距（intercept） 均值 (mean_y)
            slope = (n * sum_xy - sum_x * sum_y) / (n * sum_x2 - sum_x * sum_x);
            intercept = (sum_y - slope * sum_x) / n;
            mean_y = sum_y / n;
            data_stauts->mean = mean_y;                                // 赋值均值
            data_stauts->predicting = slope * BUFFER_SIZE + intercept; // 计算预测值
            // 计算总平方和（Total Sum of Squares）
            for (int i = 0; i < n; ++i)
            {
                data_stauts->sum_square += (data[i] - mean_y) * (data[i] - mean_y); // 计算总平方差和(单个数据-均值)^2
            }

            data_stauts->variance = data_stauts->sum_square / n; // 计算方差

            // 计算残差平方和（Residual Sum of Squares）
            for (int i = 0; i < n; ++i)
            {
                float predicted = slope * i + intercept;//计算 n_i的预测值
                data_stauts->residual += (data[i] - predicted) * (data[i] - predicted);
            }
            // 拟合优度 计算R²值 值越大，线性度越好 最大值为1
            data_stauts->r_squared = 1 - (data_stauts->residual / data_stauts->sum_square);
            return 0;
        }
        // 公共类
    public:
    /**
     * @brief 构造函数
     * @param processNoise_t 过程噪声
     * @param measurementNoise_t 测量噪声
     * @param estimatedError_t 估计误差 (初始值一般为0)
    */
        fusion(float processNoise_t, float measurementNoise_t, float estimatedError_t) // 构造函数 创建时被调用
        {
            processNoise = processNoise_t;         // 过程噪声
            measurementNoise = measurementNoise_t; // 测量噪声
            estimatedError = estimatedError_t;     // 估计误差
        }

        float EKF_Fusion(float a, float b);
    };
} // namespace fusion
